package com.zzz.web.LocationAlgo;


import com.zzz.web.Data.RSSI;
import com.zzz.web.tools.tool;

import java.util.ArrayList;
import java.util.List;

/**
 * 卡尔曼滤波算法
 */
public class KalmanFilter {


    private Integer predict;
    private Integer current;
    private Integer estimate;
    private double pdelt;
    private double mdelt;
    private double Gauss;
    private double kalmanGain;
    private final static double Q = 0.00001;
    private final static double R = 0.1;

    void  initial() {
        pdelt = 4;    //系统测量误差
        mdelt = 3;
    }

    Integer kalmanFilter(Integer oldValue, Integer value) {
        //(1)第一个估计值
        predict = oldValue;
        current = value;
        //(2)高斯噪声方差
        Gauss = Math.sqrt(pdelt * pdelt + mdelt * mdelt) + Q;
        //(3)估计方差
        kalmanGain = Math.sqrt((Gauss * Gauss) / (Gauss * Gauss + pdelt * pdelt)) + R;
        //(4)估计值
        estimate = (int) (kalmanGain * (current - predict) + predict);
        //(5)新的估计方差
        mdelt = Math.sqrt((1 - kalmanGain) * Gauss * Gauss);

        return estimate;
    }

    public RSSI Run(List<RSSI> rssis) {
        initial();
        List<RSSI> resultList = new ArrayList<>();
        RSSI tempRssi,oldRssi;
        oldRssi = rssis.get(0);
        for (int i = 0; i < rssis.size(); i++) {
            tempRssi = rssis.get(i);

            oldRssi.setAP1RSSI(kalmanFilter(oldRssi.getAP1RSSI(),tempRssi.getAP1RSSI()));
            oldRssi.setAP2RSSI(kalmanFilter(oldRssi.getAP2RSSI(),tempRssi.getAP2RSSI()));
            oldRssi.setAP3RSSI(kalmanFilter(oldRssi.getAP3RSSI(),tempRssi.getAP3RSSI()));

            resultList.add(oldRssi);
        }
        return tool.avgRSSI(resultList);
    }


}
